/**
 * @author bruno perry
 * @since 16-11-2009
 * 
 * This is an excerpt from the MICRO-GZM project (code.google.com/p/micro-gzm/)
 */
 
import math3D.Vector3D;

class math3D.Matrix3D {
	
	public var matrix:Array;
	private static var PI_CONST:Number = Math.PI;
	
	/**
	 * CONSTRUCTOR
	 * @param r11 Number
	 * @param r12 Number
	 * @param r13 Number
	 * @param r14 Number
	 * @param r21 Number
	 * @param r22 Number
	 * @param r23 Number
	 * @param r24 Number
	 * @param r31 Number
	 * @param r32 Number
	 * @param r33 Number
	 * @param r34 Number
	 * @param r41 Number
	 * @param r42 Number
	 * @param r43 Number
	 * @param r44 Number
	 */
	public function Matrix3D(r11:Number, r12:Number, r13:Number, r14:Number,
							 r21:Number, r22:Number, r23:Number, r24:Number,
							 r31:Number, r32:Number, r33:Number, r34:Number,
							 r41:Number, r42:Number, r43:Number, r44:Number) {
	 	
		matrix = new Array(16);
		
	 	if(r11){
	 		matrix[0] = r11,  matrix[1] = r12,  matrix[2] = r13,  matrix[3] = r14, 
			matrix[4] = r21,  matrix[5] = r22,  matrix[6] = r23,  matrix[7] = r24, 
			matrix[8] = r31,  matrix[9] = r32,  matrix[10] = r33, matrix[11] = r34, 
			matrix[12] = r41, matrix[13] = r42, matrix[14] = r43, matrix[15] = r44;
	 	} else {
	 		matrix[0] = 1,  matrix[1] = 0,  matrix[2] = 0,  matrix[3] = 0, 
			matrix[4] = 0,  matrix[5] = 1,  matrix[6] = 0,  matrix[7] = 0, 
			matrix[8] = 0,  matrix[9] = 0,  matrix[10] = 1, matrix[11] = 0, 
			matrix[12] = 0, matrix[13] = 0, matrix[14] = 0, matrix[15] = 1;
	 	}
	}
	/**
	 * 
	 * equals a matrix
	 * @param m Matrix3D
	 * 
	 * @return Void
	 * 
	 */
	public function equals(m:Matrix3D):Void {
		
		// Row 1 start.
		matrix[0]  = m.matrix[0];
		matrix[4]  = m.matrix[4];
		matrix[8]  = m.matrix[8];
		matrix[12] = m.matrix[12];
		// Row 2 start.
		matrix[1]  = m.matrix[1];
		matrix[5]  = m.matrix[5];
		matrix[9]  = m.matrix[9];
		matrix[13] = m.matrix[13];
		// Row 3 start.
		matrix[2]  = m.matrix[2];
		matrix[6]  = m.matrix[6];
		matrix[10] = m.matrix[10];
		matrix[14] = m.matrix[14];
		// Row 4 start.
		matrix[3]  = m.matrix[3];
		matrix[7]  = m.matrix[7];
		matrix[11] = m.matrix[11];
		matrix[15] = m.matrix[15];
	}
	/**
	 * 
	 * add a matrix
	 * @param m Matrix3D
	 * 
	 * @return Matrix3D
	 * 
	 * 
	 */
	public function add(m:Matrix3D):Matrix3D {
		
		return new Matrix3D( matrix[0] + m.matrix[0],
						   matrix[1] + m.matrix[1],
						   matrix[2] + m.matrix[2],
						   matrix[3] + m.matrix[3],
						   // Row 2 start.
						   matrix[4] + m.matrix[4],
						   matrix[5] + m.matrix[5],
						   matrix[6] + m.matrix[6],
						   matrix[7] + m.matrix[7],
						   // Row 3 start.
						   matrix[8] + m.matrix[8],
						   matrix[9] + m.matrix[9],
						   matrix[10] + m.matrix[10],
						   matrix[11] + m.matrix[11],
						   // Row 4 start.
						   matrix[12] + m.matrix[12],
						   matrix[13] + m.matrix[13],
						   matrix[14] + m.matrix[14],
						   matrix[15] + m.matrix[15] );
	};
	/**
	 * 
	 * subtracts a matrix
	 * @param m Matrix3D
	 * 
	 * @return Matrix3D
	 * 
	 */
	public function subtract(m:Matrix3D):Matrix3D {
	 	
	 	return new Matrix3D( matrix[0] - m.matrix[0],
						   matrix[1] - m.matrix[1],
						   matrix[2] - m.matrix[2],
						   matrix[3] - m.matrix[3],
						   // Row 2 start.
						   matrix[4] - m.matrix[4],
						   matrix[5] - m.matrix[5],
						   matrix[6] - m.matrix[6],
						   matrix[7] - m.matrix[7],
						   // Row 3 start.
						   matrix[8] - m.matrix[8],
						   matrix[9] - m.matrix[9],
						   matrix[10] - m.matrix[10],
						   matrix[11] - m.matrix[11],
						   // Row 4 start.
						   matrix[12] - m.matrix[12],
						   matrix[13] - m.matrix[13],
						   matrix[14] - m.matrix[14],
						   matrix[15] - m.matrix[15] );
	 };
	/**
	 * 
	 * multiply a matrix
	 * @param m Matrix3D
	 * 
	 * @return Matrix3D
	 * 
	 */
	public function multiply(m:Matrix3D):Matrix3D {
	 	
	 	var newMatrix3D:Array = new Array(16);
		var m1:Array = matrix;
		var m2:Array = m.matrix;
		
		newMatrix3D[0] = m1[0] * m2[0] + m1[1] * m2[4] + m1[2] * m2[8] + m1[3] * m2[12];
		newMatrix3D[1] = m1[0] * m2[1] + m1[1] * m2[5] + m1[2] * m2[9] + m1[3] * m2[13];
		newMatrix3D[2] = m1[0] * m2[2] + m1[1] * m2[6] + m1[2] * m2[10] + m1[3] * m2[14];
		newMatrix3D[3] = m1[0] * m2[3] + m1[1] * m2[7] + m1[2] * m2[11] + m1[3] * m2[15];

		newMatrix3D[4] = m1[4] * m2[0] + m1[5] * m2[4] + m1[6] * m2[8] + m1[7] * m2[12];
		newMatrix3D[5] = m1[4] * m2[1] + m1[5] * m2[5] + m1[6] * m2[9] + m1[7] * m2[13];
		newMatrix3D[6] = m1[4] * m2[2] + m1[5] * m2[6] + m1[6] * m2[10] + m1[7] * m2[14];
		newMatrix3D[7] = m1[4] * m2[3] + m1[5] * m2[7] + m1[6] * m2[11] + m1[7] * m2[15];

		newMatrix3D[8] = m1[8] * m2[0] + m1[9] * m2[4] + m1[10] * m2[8] + m1[11] * m2[12];
		newMatrix3D[9] = m1[8] * m2[1] + m1[9] * m2[5] + m1[10] * m2[9] + m1[11] * m2[13];
		newMatrix3D[10] = m1[8] * m2[2] + m1[9] * m2[6] + m1[10] * m2[10] + m1[11] * m2[14];
		newMatrix3D[11] = m1[8] * m2[3] + m1[9] * m2[7] + m1[10] * m2[11] + m1[11] * m2[15];

		newMatrix3D[12] = m1[12] * m2[0] + m1[13] * m2[4] + m1[14] * m2[8] + m1[15] * m2[12];
		newMatrix3D[13] = m1[12] * m2[1] + m1[13] * m2[5] + m1[14] * m2[9] + m1[15] * m2[13];
		newMatrix3D[14] = m1[12] * m2[2] + m1[13] * m2[6] + m1[14] * m2[10] + m1[15] * m2[14];
		newMatrix3D[15] = m1[12] * m2[3] + m1[13] * m2[7] + m1[14] * m2[11] + m1[15] * m2[15];
		
		return new Matrix3D( newMatrix3D[0], newMatrix3D[1], newMatrix3D[2], newMatrix3D[3],
					   	   newMatrix3D[4], newMatrix3D[5], newMatrix3D[6], newMatrix3D[7],
					  	   newMatrix3D[8], newMatrix3D[9], newMatrix3D[10],newMatrix3D[11],
					  	   newMatrix3D[12],newMatrix3D[13],newMatrix3D[14],newMatrix3D[15] );
	}
	/**
	 * 
	 * identify the matrix
	 * 
	 * @return Void
	 * 
	 */
	public function Identity():Void {
		
		/*
		|1000|
		|0100|
		|0010|
		|0001|
		*/
		matrix[0]  = 1.0;
		matrix[1]  = 0.0;
		matrix[2]  = 0.0;
		matrix[3]  = 0.0;
		matrix[4]  = 0.0;
		matrix[5]  = 1.0;
		matrix[6]  = 0.0;
		matrix[7]  = 0.0;
		matrix[8]  = 0.0;
		matrix[9]  = 0.0;
		matrix[10] = 1.0;
		matrix[11] = 0.0;
		matrix[12] = 0.0;
		matrix[13] = 0.0;
		matrix[14] = 0.0;
		matrix[15] = 1.0;
	}
	/**
	 * 
	 * translate the matrix by values (xyz)
	 * @param x Number
	 * @param y Number
	 * @param z Number
	 * 
	 * @return Void
	 * 
	 */
	public function Translate(x:Number,y:Number,z:Number):Void {
	 	
	 	/*
		|1000|
		|0100|
		|0010|
		|xyz1|
		*/
		
		matrix[12] = x;
		matrix[13] = y;
		matrix[14] = z;

		matrix[15] = 1.0;
	}
	/**
	 * 
	 * translate the matrix by values (xyz)
	 * @param x Number
	 * @param y Number
	 * @param z Number
	 * 
	 * @return Void
	 * 
	 */
	public function translate(pos:Vector3D):Matrix3D {
	 	
	 	/*
		|1000|
		|0100|
		|0010|
		|xyz1|
		*/
		
		matrix[12] = pos.x;
		matrix[13] = pos.y;
		matrix[14] = pos.z;

		matrix[15] = 1.0;
		
		return new Matrix3D( matrix[0], matrix[1], matrix[2], matrix[3],
					   	     matrix[4], matrix[5], matrix[6], matrix[7],
					  	     matrix[8], matrix[9], matrix[10],matrix[11],
					  	     matrix[12],matrix[13],matrix[14],matrix[15] );
	}
	/**
	* 
	* multiply a vector by the matrix
	* @param v Vector3D
	* 
	* @return Vector3D
	*	
	*/
	public function Vector3DMatrix3DMultiply(v:Vector3D):Vector3D {
		
		var out:Vector3D = new Vector3D();

		out.x = (v.x * matrix[0]) + (v.y * matrix[4]) + (v.z * matrix[8]) + matrix[12];
		out.y = (v.x * matrix[1]) + (v.y * matrix[5]) + (v.z * matrix[9]) + matrix[13];
		out.z = (v.x * matrix[2]) + (v.y * matrix[6]) + (v.z * matrix[10]) + matrix[14];

		return out;
	}
	/**
	* 
	* scale the matrix by a vector
	* @param scale Vector3D
	* 
	* @return Void
	*	
	*/
	public function Scale(scale:Vector3D):Void {
		
		matrix[0] = scale.x;
		matrix[5] = scale.y;
		matrix[10] = scale.z;
	}
	/**
	* 
	* set the rotation matrix radians by value(xyz)
	* @param x Number
	* @param y Number
	* @param z Number
	* 
	* @return Void
	*	
	*/
	public function SetRotationRadians(x:Number,y:Number,z:Number):Void {
		
		var cosX:Number = Math.cos(x);
		var cosY:Number = Math.cos(y);
		var cosZ:Number = Math.cos(z);
		var sinX:Number = Math.sin(x);
		var sinY:Number = Math.sin(y);
		var sinZ:Number = Math.sin(z);

		matrix[0] = cosY * cosZ;
		matrix[1] = cosY * sinZ;
		matrix[2] = (-sinY);

		matrix[4] = (sinX * sinY) * cosZ - cosX * sinZ;
		matrix[5] = (sinX * sinY) * sinZ + cosX * cosZ;
		matrix[6] = sinX * cosY;

		matrix[8] = (cosX * sinY) * cosZ + sinX * sinZ;
		matrix[9] = (cosX * sinY) * sinZ - sinX * cosZ;
		matrix[10] = cosX * cosY;
	}
	/**
	* 
	* rotate the matrix by angle
	* @param angle Number
	* @param x int
	* @param y int
	* @param z int
	* 
	* @return Void
	*	
	*/
	public function Rotate(angle:Number,x:Number,y:Number,z:Number):Void {
		
		angle = DEG_TO_RAD(angle);
		
		var cosAngle:Number = Math.cos(angle);
		var sineAngle:Number = Math.sin(angle);

		if (z)
		{
			matrix[0] = cosAngle;
			matrix[1] = sineAngle;
			matrix[4] = -sineAngle;
			matrix[5] = cosAngle;
		}
		if (y)
		{
			matrix[0]  = cosAngle;
			matrix[2]  = -sineAngle;
			matrix[8]  = sineAngle;
			matrix[10] = cosAngle;
		}
		if (x)
		{
			matrix[5]  = cosAngle;
			matrix[6]  = sineAngle;
			matrix[9]  = -sineAngle;
			matrix[10] = cosAngle;
		}
	}
	/**
	* 
	* rotate the matrix by angle
	* @param angle Number
	* @param x int
	* @param y int
	* @param z int
	* 
	* @return Void
	*	
	*/
	public function rotate(angle:Number,axis:Vector3D):Matrix3D {
		
		angle = DEG_TO_RAD(angle);

		var cosAngle:Number = Math.cos(angle);
		var sineAngle:Number = Math.sin(angle);

		if (axis.z == 1)
		{
			matrix[0] = cosAngle;
			matrix[1] = sineAngle;
			matrix[4] = -sineAngle;
			matrix[5] = cosAngle;
		}
		if (axis.y == 1)
		{
			matrix[0]  = cosAngle;
			matrix[2]  = -sineAngle;
			matrix[8]  = sineAngle;
			matrix[10] = cosAngle;
		}
		if (axis.x == 1)
		{
			matrix[5]  = cosAngle;
			matrix[6]  = sineAngle;
			matrix[9]  = -sineAngle;
			matrix[10] = cosAngle;
		}
		
		return new Matrix3D( matrix[0], matrix[1], matrix[2], matrix[3],
					   	     matrix[4], matrix[5], matrix[6], matrix[7],
					  	     matrix[8], matrix[9], matrix[10],matrix[11],
					  	     matrix[12],matrix[13],matrix[14],matrix[15] );
	}		
	/**
	* 
	* rotate the matrix(axis) by angle
	* @param angle Number
	* @param axis Vector3D
	* 
	* @return Void
	*	
	*/
	public function RotateAxis(angle:Number,axis:Vector3D):Void {
		
		axis.Normalize(new Vector3D(),new Vector3D(),new Vector3D());

		var sinAngle:Number = Math.sin((PI_CONST * angle) / 180);
		var cosAngle:Number = Math.cos((PI_CONST * angle) / 180);
		var oneSubCos:Number = 1.0 - cosAngle;

		matrix[0] = (axis.x * axis.x) * oneSubCos + cosAngle;
		matrix[4] = (axis.x * axis.y) * oneSubCos - (axis.z * sinAngle);

		matrix[1] = (axis.y * axis.x) * oneSubCos + (sinAngle * axis.z);
		matrix[5] = (axis.y * axis.y) * oneSubCos + cosAngle;
		matrix[9] = (axis.y * axis.z) * oneSubCos - (axis.x * sinAngle);

		matrix[2] = (axis.z * axis.x) * oneSubCos - (axis.y * sinAngle);
		matrix[6] = (axis.z * axis.y) * oneSubCos + (axis.x * sinAngle);
		matrix[10] = (axis.z * axis.z) * oneSubCos + cosAngle;
	}
	/**
	* 
	* rotate the matrix(x) by angle
	* @param angle Number
	* 
	* @return Void
	*	
	*/
	public function RotateX(angle:Number):Matrix3D {
		
		matrix[5] = -Math.cos(PI_CONST * angle / 180);
		matrix[6] = Math.sin(PI_CONST * angle / 180);
		matrix[9] = -matrix[6];
		matrix[10] = matrix[5];
		
		return this;
	}
	/**
	* 
	* rotate the matrix(y) by angle
	* @param angle Number
	* 
	* @return Void
	*	
	*/
	public function RotateY(angle:Number):Matrix3D {
		
		matrix[0] = Math.cos(PI_CONST * angle / 180);
		matrix[2] = -Math.sin(PI_CONST * angle / 180);

		matrix[8] = -matrix[2];
		matrix[10] = matrix[0];
		
		return this;
	}
	/**
	* 
	* rotate the matrix(z) by angle
	* @param angle Number
	* 
	* @return Void
	*	
	*/
	public function RotateZ(angle:Number):Matrix3D {
		
		matrix[0] = Math.cos(PI_CONST * angle / 180);
		matrix[1] = Math.sin(PI_CONST * angle / 180);

		matrix[4] = -matrix[1];
		matrix[5] = matrix[0];
		
		return this;
	}
	/**
	 * MATH
	 */
	private function DEG_TO_RAD(angle:Number):Number {
		
		return (angle/180) * Math.PI;
	}
}